/**
 * 
 */
package robot;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Sound;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;

/**
 * @author zsol
 *
 */
public class Main {

	static float x, y;
	static int dir;
	
	static DataInputStream dis;
	static DataOutputStream dos;
	
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		LCD.drawString(   "Lajos", 6, 3, true);
		LCD.drawString("The Explorer", 3, 4);
		
		Config.configurePorts();
		
		//Config.nav.calibrateCompass();
		
		LCD.drawString("Waiting for connection", 0, 0);
		
		BTConnection conn = null;
		conn = Bluetooth.waitForConnection();
		/*while (conn == null) {
			conn = Bluetooth.waitForConnection(500, 0, Bluetooth.defaultPin);
		}*/
		
		dis = conn.openDataInputStream();
		dos = conn.openDataOutputStream();
		
		byte signal = 0;
		
		while (signal != 1) {
			LCD.drawString("Rec: " + signal, 1, 1);
			LCD.drawString("Waiting for start signal", 0, 0);
			
			try {
				signal = dis.readByte();
			} catch (IOException e1) {
				LCD.drawString("Error during read", 0, 0);
				Button.waitForPress();
				signal = 1;
			}
		}

		boolean obs = isObstacle();
		boolean end = false;
		int cnt = 0;
		Config.nav.setPosition(0, 0, 90);
		dir = 1;
		Config.csSensor.resetCartesianZero();
		while (!end) {
			try {
				getNewDirection();
				LCD.drawInt(dir, 1, 6);
			} catch (Exception e) {
				LCD.drawString("I'm trapped!", 1,1, true);
				end = true;
				Button.waitForPress();
			}
			
			obs = isObstacle();
			x = Config.nav.getX();
			y = Config.nav.getY();
			move(dir);
	
			LCD.drawString("Cur: " , 1, 1);
			LCD.drawString("" + x, 1, 2);
			LCD.drawString("" + y, 1, 3);
			
			try {
				cnt = dis.available();
				if (cnt > 0) {
					signal = dis.readByte();
				}
				if (signal == 0) {
					end = true;
				}
			} catch (IOException e) {
				cnt = 0;
			}

		}
		
		try {
			dis.close();
			dos.close();
			conn.close();
		} catch (IOException e) {
		}
		
	}
	
	public static void sendcoords(float x, float y, int dir) {
		switch (dir) {
		case 0:
			x += 10;
			break;
		case 1:
			y += 10;
			break;
		case 2:
			x -= 10;
			break;
		case 3:
			y -= 10;
			break;
		}
		try {
			dos.writeFloat(x);
			dos.writeFloat(y);
		} catch (IOException e) {
			LCD.drawString("Error during write", 0, 0);
		}
	}
	
	public static void sendcoords(float x, float y) {
		sendcoords(x, y, -1);
	}
	
	public static void move(int direction) {
		switch (direction) {
		case 0:
			Config.nav.goTo(x + 10, y);
			break;
		case 1:
			Config.nav.goTo(x, y + 10);
			break;
		case 2:
			Config.nav.goTo(x - 10, y);
			break;
		case 3:
			Config.nav.goTo(x, y - 10);
			break;
		}
	}
	
	public static void getNewDirection() throws Exception {
		Sound.beep();
		x = Config.nav.getX();
		y = Config.nav.getY();
		Config.nav.rotate(90);
		moddir(1);
		if (isObstacle()) {
			Sound.buzz();
			Config.nav.rotate(-180);
			moddir(-2);
			if (isObstacle()) {
				Sound.buzz();
				Config.nav.rotate(-90);
				moddir(-1);
				if (isObstacle()) {
					Sound.buzz();
					Config.nav.rotate(180);
					moddir(2);
					if (isObstacle()) {
						Sound.beepSequence();
						throw new Exception();
					}
				}
			}
		}
	}
	
	public static void moddir(int delta) {
		dir += delta;
		while (dir < 0 || dir > 3) {
			if (dir < 0) {
				dir += 4;
			}
			if (dir > 3) {
				dir -= 4;
			}
		}
		LCD.drawInt(dir, 1, 6);
	}
	
	public static boolean isObstacle() {
		return Config.usSensor.getDistance() < 20;
	}

}
